#! /usr/bin/env python3
# -*- coding: utf-8 -*

import rospy
import actionlib # action服务器
from move_base_msgs.msg import MoveBaseAction,MoveBaseActionGoal # move_base服务器接口
from auto_charging.srv import *
from relative_move.srv import SetRelativeMoveRequest,SetRelativeMove
from rei_robot_base.msg import CarData
from rei_robot_base.srv import *
from std_srvs.srv import *
from robot_audio.srv import *
from ar_pose.srv import *
from functools import wraps
from arm_controller.srv import *
from dobot.srv import *
import tf
import time
import os
import sys

class smartLogistics():
    def __init__(self,node_name):
        rospy.init_node("smart_logistics",anonymous=True)
        self.node_name = node_name
        # self.audio_file = rospy.get_param("~audio_file")
        self.point_list = dict()
        self.task_list = list()
        self.__get_point__()
        # self.__get_task__()
        # 创建一个客户端，设置服务名，action类型
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self.tts_client = rospy.ServiceProxy("voice_tts",robot_tts)
        self.collect_client = rospy.ServiceProxy("voice_collect",Collect)
        self.aiui_client = rospy.ServiceProxy("voice_aiui",robot_semanteme)
        self.iat_client = rospy.ServiceProxy("voice_iat",robot_iat)
        self.relmove_client = rospy.ServiceProxy("/relative_move",SetRelativeMove)
        self.track_client = rospy.ServiceProxy("/track",Track)
        self.pickar_client = rospy.ServiceProxy("/pick_ar",PickPlace)
        self.armmove_client = rospy.ServiceProxy("/DobotServer/SetPTPCmd",SetPTPCmd)
        self.clearErr_client = rospy.ServiceProxy("/DobotServer/ClearAllAlarmsState",ClearAllAlarmsState)
        self.setHome_client = rospy.ServiceProxy("/DobotServer/SetHOMECmd",SetHOMECmd)
        self.place_client = rospy.ServiceProxy("/DobotServer/SetEndEffectorSuctionCup",SetEndEffectorSuctionCup)
        # 取消正在运行的目标
        self.move_base.cancel_goal()

    def func_header():
        def get_param(func):
            @wraps(func)
            def wrapper(self,*args,**kwargs):
                try:
                    rospy.loginfo(f"info:开始执行 {func.__name__}")
                    s_time = time.time()
                    result = func(self,*args,**kwargs)
                    e_time = time.time()
                    # rospy.loginfo(f"运行时间:{e_time - s_time}:{func.__name__}")
                    # rospy.loginfo(f"info:运行时间 ({e_time - s_time}:{func.__name__})")
                    return result
                except:
                    rospy.logerr(f'error:函数{func.__name__}出现了错误')
                    return False
            return wrapper
        return get_param

    def __get_point__(self):
        """获取导航点数据"""
        goal_list = rospy.get_param(f"/navigation_point/navigation_target")
        for goal in goal_list:
            point = rospy.get_param(f"/navigation_point/navigation_target/{goal}")
            points = MoveBaseActionGoal()
            points.goal.target_pose.header.frame_id = "map" # 使用map的frame定义goal的frame id
            # 设置导航点位置
            points.goal.target_pose.pose.position.x = point["position"]["x"]
            points.goal.target_pose.pose.position.y = point["position"]["y"]
            points.goal.target_pose.pose.orientation.z = point["orientation"]["z"]
            points.goal.target_pose.pose.orientation.w = point["orientation"]["w"]
            self.point_list[goal] = points
            rospy.loginfo(f"init:导入导航点 {goal}")
        return self.point_list

    @func_header()
    def voice_tts(self,text):
        rospy.loginfo(f'info:等待语音合成服务')
        rospy.wait_for_service("voice_tts")
        rospy.loginfo(f'info:等待语音合成服务已开启')        
        req = robot_ttsRequest()
        req.text = text
        req.play = 1
        self.tts_client.call(req)
        return True

    @func_header()
    def place(self,mode):
        req = SetEndEffectorSuctionCupRequest()
        req.enableCtrl = 1
        req.suck = mode
        req.isQueued = 1
        self.place_client.call(req)
        rospy.loginfo(f'info:已经设置吸盘状态为：{mode}')
        return True

    @func_header()
    def voice_collect(self):
        rospy.loginfo(f'info:等待语音采集服务')
        rospy.wait_for_service("voice_collect")
        rospy.loginfo(f'info:等待语音采集服务已经开启')
        req = CollectRequest()
        req.collect_flag = True
        resp = self.collect_client.call(req)
        return resp.voice_filename

    @func_header()
    def voice_aiui(self,text):
        rospy.loginfo(f'info:等待AIUI服务')
        rospy.wait_for_service("/voice_aiui")
        rospy.loginfo(f'info:AIUI服务已经开启')
        req = robot_semantemeRequest()
        req.mode = 1
        req.textorpath = text
        resp = self.aiui_client.call(req)
        return resp

    @func_header()
    def relmove(self,x,y,theta):
        rospy.loginfo(f'info:等待相对移动服务')
        rospy.wait_for_service("/relative_move")
        rospy.loginfo(f'info:相对移动服务已开启')
        req = SetRelativeMoveRequest()
        req.goal.x = x
        req.goal.y = y
        req.goal.theta = theta
        req.global_frame = 'map'
        req.is_omni = True
        resp = self.relmove_client.call(req)
        return resp.success

    @func_header()
    def track(self,id,goal_dist):
        rospy.loginfo(f'info:等待底盘对准服务')
        rospy.wait_for_service("/track")
        rospy.loginfo(f'info:底盘对准服务已经开启')
        req = TrackRequest()
        req.ar_id = id
        req.goal_dist = goal_dist
        resp = self.track_client.call(req)
        return resp.success

    @func_header()
    def pick_ar(self,id,x=0,y=0,z=0):
        rospy.loginfo(f'info:等待手臂二维码抓取服务')
        rospy.wait_for_service("/pick_ar")
        rospy.loginfo(f'info:手臂二维码抓取服务已经开启')
        req = PickPlaceRequest()
        req.number = id
        req.mode = 1
        req.pose.position.x = x
        req.pose.position.y = y
        req.pose.position.z = z
        resp = self.pickar_client.call(req)
        return resp.success

    @func_header()
    def clearErr(self):
        resp = self.clearErr_client.call()
        return resp.result

    @func_header()
    def arm_move(self,x,y,z,r=0.0,mode=1):
        rospy.loginfo(f'info:Wait for action to activate')
        rospy.wait_for_service("/DobotServer/SetPTPCmd")
        req = SetPTPCmdRequest()
        req.ptpMode = mode
        req.x = x
        req.y = y
        req.z = z
        req.r = r
        resp = self.armmove_client.call(req)
        rospy.sleep(1)
        return resp.result

    @func_header()
    def voice_iat(self,audiopath):
        rospy.loginfo(f'info:Wait for action to activate')
        rospy.wait_for_service("/voice_iat")
        req = robot_iatRequest()
        req.audiopath = audiopath
        resp = self.iat_client.call(req)
        return resp.text

    @func_header()
    def nav_goal(self,goal):
        rospy.loginfo("Wait for action to activate")  
        self.move_base.wait_for_server() # 等待服务开启
        rospy.loginfo("Action is active")  
        goal.goal.target_pose.header.stamp = rospy.Time.now() # 设置时间戳
        self.move_base.send_goal(goal.goal)  
        self.move_base.wait_for_result() # 等待服务结果
        state = self.move_base.get_state() # 获取导航状态
        if state == actionlib.GoalStatus.SUCCEEDED:
            rospy.loginfo("Goal succeeded!")
            return True
        else:
            rospy.loginfo("Goal failed!")
            return False
    
    @func_header()
    def init_arm(self):
        self.setHome_client.call()
        rospy.sleep(2)
        self.clearErr_client.call()
        return True

    def get_transform(self, parent_frame,child_frame):
        listener = tf.TransformListener()
        # parent_frame = 'link_2'
        # child_frame = 'ar_marker_' + str(id)
        try:
            # 等待获取变换信息
            listener.waitForTransform(parent_frame, child_frame, rospy.Time(0), rospy.Duration(10.0))
            # 获取变换信息
            (trans, rot) = listener.lookupTransform(parent_frame, child_frame, rospy.Time(0))
            return trans, rot
        except:
            rospy.logerr("Failed to get transform from %s to %s", parent_frame, child_frame)
            return None, None

class robot_run():
    def __init__(self,box=0):
        self.sl = smartLogistics("smart_logistics")
        self.city = ["武汉","荆州","十堰","宜昌","咸宁","黄石","随州","恩施"]
        self.Product = ["港饼","玉露","桂花糕","脐橙","贡米","武昌鱼","鱼糕","未定义","泡泡青"]
        # obj = {"湖南":"武昌鱼","福建":"恩施玉露","河南":"脐橙"}
        # obj = {"黄鹤楼":"武昌鱼","恩施":"恩施玉露","宜昌":"脐橙"}
        # 初始点sl.goto_position(0,130,0)
        # 工作台观测点：x: 1.0, y: -200.0, z: 45.0
        # 放置点1：x: 19.0, y: 178.0, z: -59.0
        # 放置点2：x: 69.0, y: 178.0, z: -59.0
        # self.sl.place(0)
        self._cargo_boxes = {
                                1:{
                                    "point":[20.0,178.0,-30],
                                    "state":False,
                                    "product":0
                                },
                                2:{
                                    "point":[70.0,178.0,-30],
                                    "state":False,
                                    "product":0
                                }
                            }
        self.warehouse = 0
        self.storage_size = box
        self.is_track = False

    def init_arm(self):
        resp = self.sl.setHome_client.call()
        rospy.sleep(10)
        if resp.result != 0:
            self.rlog('初始化失败')
            return False
        resp = self.sl.clearErr_client.call()
        if resp.result != 0:
            self.rlog('初始化失败')
            return False
        self.rlog('初始化成功')
        self.sl.arm_move(0,130,0)
        rospy.sleep(2)
        return True

    def init_storage(self):
        self.storage_size = 0

    def rlog(self,text,mode=0):
        if mode == 1:
            rospy.logerr(text)
        if mode == 0:
            rospy.loginfo(text)

    def move_arm(self,pose,r=0):
        self.sl.clearErr()
        ret = self.sl.arm_move(pose[0],pose[1],pose[2],r)
        if ret < 0:
            self.rlog("目标点错误")
            self.sl.clearErr()
            self.sl.arm_move(0,130,0)
            self.sl.clearErr()
            return False

    def In_storage(self,pose,boxNum):
        if self._cargo_boxes[boxNum]["state"] == True:
            self.rlog(f"<货物入库失败>:中转箱-{boxNum}已满",mode=1)
            return False
        self.sl.clearErr()
        self.sl.place(0)
        # 抓取位置
        ret = self.sl.arm_move(pose[0],pose[1],-72,mode=0)
        if ret < 0:
            self.sl.clearErr()
            self.rlog("目标点错误")
            self.sl.clearErr()
            rospy.sleep(2)
            self.sl.arm_move(0,130,0,mode=0)
            return False
        self.sl.place(1)
        rospy.sleep(0.5)
        # self.sl.arm_move(0,200,0,mode=1)
        # 入库
        storage_pose = self._cargo_boxes[boxNum]["point"]
        self.sl.arm_move(storage_pose[0],storage_pose[1],storage_pose[2],mode=0)
        self._cargo_boxes[boxNum]["state"] = True
        self.sl.place(0)
        # 回到安全位置
        self.sl.arm_move(0,200,0)
        self.sl.arm_move(0,130,0,mode=1)
        self.storage_size += 1
        self.rlog("放置成功")
        return True

    def Out_storage(self,pose,boxNum):
        if self._cargo_boxes[boxNum]["state"] == False:
            self.rlog(f"<货物出库失败>中转箱-{boxNum}为空",mode=1)
            return False
        self.sl.place(0)
        self.sl.arm_move(0,200,0)
        storage_pose = self._cargo_boxes[boxNum]["point"]
        high = -45
        self.sl.arm_move(storage_pose[0],storage_pose[1],high,mode=0)
        self.storage_size += -1
        self.sl.place(1)
        ret = self.sl.arm_move(pose[0],pose[1],pose[2],mode=0)
        self.sl.place(0)
        ret = self.sl.arm_move(pose[0],pose[1],pose[2]+20,mode=0)
        self._cargo_boxes[boxNum]["state"] = False
        if ret < 0:
            self.sl.clearErr()
            self.rlog("目标点错误")
            self.sl.clearErr()
            rospy.sleep(2)
            self.sl.arm_move(0,130,0,mode=0)
            return False
        self.sl.arm_move(0,130,0,mode=1)
        return True

    def move_car(self,landmark_name,track=False):
        if not landmark_name in self.sl.point_list:
            self.rlog('未知的地点名')
            return False
        if self.is_track == True:
            self.sl.relmove(-0.2,0,0)
            self.is_track =False
        point = self.sl.point_list[landmark_name]
        self.sl.nav_goal(point)
        if track == True:
            self.sl.track(0,0.4)
            self.sl.relmove(0.20,0,0)
            self.is_track = True
        rospy.sleep(1)
        return True

    def get_mark(self,id):
        rospy.sleep(2)
        link = "/ar_marker_" + str(id)
        tran,rot = self.sl.get_transform("/link_2",link)
        if tran == None:
            self.rlog('没有识别到二维码')
            return None
        return [tran[0]*1000,tran[1]*1000,tran[2]*1000]
        
    def NLP(self,resp):
        question = resp.question
        question = question.replace("武当山","十堰")
        sp_ques = question.replace(" ","").split("//")+["",""]
        print(sp_ques)
        if len(sp_ques)<=0:
            self.rlog("未识别到分词")
            return False,[]
        # 查找
        mode = 0
        city = ""
        text = ""
        
        for i in range(len(sp_ques)-2):
            sp = sp_ques[i].split("/")
            sp_1 = sp_ques[i+1].split("/")
            sp_2 = sp_ques[i+2].split("/")
            if mode == 0:
                text += sp[0]
                if len(sp_1) == 2 and sp_1[1][0] == "V":
                    if len(sp) == 2 and sp[1][:2] == "NS":
                        print("mode 1"+sp[0])
                        mode = 1
                        city = sp[0]
                        text = ""
                    elif len(sp) == 2 and sp[1][:2] == "NN":
                        if len(sp_2) == 2 and sp_2[1][:2] == "NS":
                            print("mode 2"+sp_2[0])
                            mode = 2
                            city = sp_2[0]
                            break
            elif mode == 1:
                text += sp[0]
        if mode == 0:
            self.rlog("语句识别失败",mode=1)
            return False,[]
        print(text,city)
        # 查找城市是否注册
        is_city_true = False
        for in_city in self.city:
            if city.find(in_city) >= 0:
                is_city_true = True
                city = in_city
                break
        if is_city_true == False:
            self.rlog("这个城市还未开通服务")
            return True,[1,"这个城市还未开通服务"]
        is_product = False
        for in_product in self.Product:
            if text.find(in_product) >= 0:
                text = in_product
                is_product = True
        if is_product == False:
            self.rlog("这个货物现在还没有")
            return True,[1,"这个货物现在还没有"]
        return True,[2,mode,city,text]
    
    def AI_robot(self,text):
        resp = self.sl.voice_aiui(text)
        try:
            if resp.tech == "user":
                if resp.intent == "robot_control":
                    # 判断单位
                    if resp.slots_value[2] == '厘米':
                        unit = 0.01
                    elif resp.slots_value[2] == '毫米':
                        unit = 0.001
                    else:
                        unit = 1
                    # 需要移动的距离
                    distance = float(resp.slots_value[1])*unit
                    # 控制机器人移动
                    self.sl.voice_tts(resp.anwser)
                    if resp.slots_value[0] == '前进':
                        self.sl.relmove(distance,0,0)
                    elif resp.slots_value[0] == '后退':
                        self.sl.relmove(-distance,0,0)
                    elif resp.slots_value[0] == '左移':
                        self.sl.relmove(0,distance,0)
                    elif resp.slots_value[0] == '右移':
                        self.sl.relmove(0,-distance,0)
                    elif resp.slots_value[0] == '左转':
                        self.sl.relmove(0,0,distance*3.1415926/180)
                    elif resp.slots_value[0] == '右转':
                        self.sl.relmove(0,0,-distance*3.1415926/180)
                elif resp.intent == "robot_nav":
                    self.sl.voice_tts(resp.anwser)
                    self.move_car(resp.slots_value[1])
                    
                    return True
            else:
                ret,desk = self.NLP(resp)
                print(ret,desk)
                if ret:
                    if desk[1] == 1:
                        self.rlog("进入模式一")
                        boxNum = 0
                        for i in self._cargo_boxes:
                            if self._cargo_boxes[i]["state"] == False:
                                boxNum = i
                                break
                        if boxNum == 0:
                            self.rlog("中转箱已满",1)
                            self.sl.voice_tts("中转箱已满")
                            return False
                        res = self.move_car(desk[2],track=True)
                        if res:
                            self.move_arm([50,-200,50])
                            res = self.get_mark(self.Product.index(desk[3])+1)
                            if res != None:
                                self.In_storage(res,boxNum)
                                self._cargo_boxes[boxNum]["state"] = True
                                self._cargo_boxes[boxNum]["product"] = self.Product.index(desk[3])+1
                                self.sl.voice_tts(f"已经将{desk}放入，中转箱{boxNum}")
                                return True
                            else:                                
                                self.move_arm([0,130,0])
                                self.rlog(f"没有检测到{desk[3]}",1)
                                self.sl.voice_tts(f"没有检测到{desk[3]}")
                                return False
                        else:
                            self.rlog(f"移动到{desk[1]}失败")          
                            return False                 
                    elif desk[1] == 2:
                        self.rlog("进入模式二")
                        boxNum = 0
                        for i in self._cargo_boxes:
                            if self._cargo_boxes[i]["product"] == self.Product.index(desk[3]) + 1:
                                boxNum = i
                                self.move_car(desk[2],track=True)
                                self.Out_storage([0,-200,-65],boxNum)
                                self._cargo_boxes[boxNum]["product"] = 0
                                self.sl.voice_tts(f"已经将{desk[3]}从中转箱{boxNum}取出")
                                return True
                else:
                    self.sl.voice_tts(resp.anwser)
                    return True
        except:
            self.rlog("服务调用失败")
            return False
        

def run():
    robot = robot_run()
    robot.sl.place(0)
    robot.move_arm([0,130,0])
    robot.move_car("武汉")
    robot.move_car("随州",track=True)
    # 抓取
    robot.move_arm([0,-250,50])
    rec = robot.get_mark(robot.Product.index("泡泡青") + 1)
    if rec != None:
        robot.In_storage(rec,1)
    else:
        robot.sl.place(0)
        robot.move_arm([0,130,0])
    robot.move_car("十堰",track=True)
    # 放置
    robot.Out_storage([0,-180,-65],1)
    # 抓取
    robot.move_arm([0,-250,50])
    rec = robot.get_mark(robot.Product.index("贡米") + 1)
    if rec != None:
        robot.In_storage(rec,2)
    else:
        robot.sl.place(0)
        robot.move_arm([0,130,0])
    robot.move_car("神农架",track=True)
    # 放置
    robot.Out_storage([0,-180,-65],2)
    robot.move_car("zero")
    
def run_voice():
    robot.init_arm()
    robot.AI_robot("把武昌鱼放在武汉")
    robot.move_car("宜昌")
    robot.init_arm()
    while not rospy.is_shutdown():
        rospy.sleep(1)
        robot = robot_run()
        robot.sl.voice_tts("有什么需要帮助的吗")
        voice = robot.sl.voice_collect()
        text = robot.sl.voice_iat(voice)
        robot.AI_robot(text)

def run2():
    robot = robot_run()
    robot.sl.place(0)
    robot.move_arm([0,130,0])
    robot.move_car("宜昌")
    robot.move_car("荆州",track=True)
    # 抓取
    robot.move_arm([0,-250,50])
    rec = robot.get_mark(robot.Product.index("鱼糕") + 1)
    if rec != None:
        robot.In_storage(rec,1)
    else:
        robot.sl.place(0)
        robot.move_arm([0,130,0])
    robot.move_car("黄石",track=True)
    # 放置
    robot.Out_storage([0,-180,-65],1)
    robot.move_car("zero")

def run_init():
    robot = robot_run()
    robot.init_arm()

if __name__ == "__main__":
    try:
        # run()
        # run2()
        run_init()
    except rospy.ROSInterruptException:
        rospy.loginfo("-----------------------------------------------")